#include <cstdio>
#include "SimpleVectorMatrix.h"
#include "MarkerTracker.h"
#include "ObjectDrawingFunctions.h"
#include "SnowmanFight.h"
#include <GL/glut.h>
#include <GL/gl.h>

const int TIMER_DELAY = 1000 / 30;
const float BALL_SPEED = 0.4;

FlyingBall flyingBallData;

void drawFlyingBall()
{
	if (flyingBallData.count_left > 0) 
	{
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glTranslatef(flyingBallData.pos[0], flyingBallData.pos[1], flyingBallData.pos[2]);
		glColor4f(1, 1, 1, 1);
		glutSolidSphere(0.005, 10, 10);
	}
}

void progressFlyingBall(int x)
{
	flyingBallData.pos[0] += flyingBallData.step[0];
	flyingBallData.pos[1] += flyingBallData.step[1];
	flyingBallData.pos[2] += flyingBallData.step[2];
	
	--flyingBallData.count_left;

	if (flyingBallData.count_left > 0)
		glutTimerFunc(TIMER_DELAY, progressFlyingBall, 0);

	glutPostRedisplay();
}

void throwBall(float *p1, float *p2)
{
	if (flyingBallData.count_left > 0)
		return;

	flyingBallData.pos[0] = p1[0]; flyingBallData.pos[1] = p1[1]; flyingBallData.pos[2] = p1[2];
	
	float distance = sqrt(pow(p1[0]-p2[0], 2) + pow(p1[1]-p2[1], 2) + pow(p1[2]-p2[2], 2));
	int count = distance / (BALL_SPEED * (TIMER_DELAY/1000.0));

	flyingBallData.step[0] = (p2[0]-p1[0])/count;
	flyingBallData.step[1] = (p2[1]-p1[1])/count;
	flyingBallData.step[2] = (p2[2]-p1[2])/count;

	flyingBallData.count_left = count;
	flyingBallData.count = count;

	progressFlyingBall(0);
}


void displaySnowmenLookingAtEachOther()
{
	float *s1_pose_matrix = NULL, *s2_pose_matrix = NULL;

	std::map<short, float*>::iterator s1 = markers_map.find(0x005a);
	std::map<short, float*>::iterator s2 = markers_map.find(0x0272);

	if (s1 != markers_map.end())
		s1_pose_matrix = s1->second;

	if (s2 != markers_map.end())
		s2_pose_matrix = s2->second;

	if (s1_pose_matrix != NULL && s2_pose_matrix != NULL)
	{
		// throw the ball
		
		float p1[3] = {s1_pose_matrix[3], s1_pose_matrix[7], s1_pose_matrix[11]};
		float p2[3] = {s2_pose_matrix[3], s2_pose_matrix[7], s2_pose_matrix[11]};

		//float a, x, y, z;
		//matrix2axis_angle4(s1_pose_matrix, a, x, y, z);
		//p1[0] += x*0.03; p1[1] += y*0.03; p1[2] += z*0.03;

		//matrix2axis_angle4(s2_pose_matrix, a, x, y, z);
		//p2[0] += x*0.03; p2[1] += y*0.03; p2[2] += z*0.03;

		if (rand() % 2 > 0.5)
			throwBall(p1, p2);
		else
			throwBall(p2, p1);

		// Rotate the snowmen, so that they are looking at each other
		float x1[2] = {s1_pose_matrix[1], s1_pose_matrix[5]};
		float x2[2] = {s2_pose_matrix[1], s2_pose_matrix[5]};

		float c1[2] = {s1_pose_matrix[3], s1_pose_matrix[7]};
		float c2[2] = {s2_pose_matrix[3], s2_pose_matrix[7]};

		float c[2] = {c2[0]-c1[0], c2[1]-c1[1]};
		float cn[2] = {-c[1], c[0]};

		float a1 = 0, a2 = 0;

		if (dot(x1, cn, 2) > 0)
			a1 =  angleBetweenVectors2(x1, c);
		else
			a1 = -angleBetweenVectors2(x1, c);

		if (dot(x2, cn, 2) > 0)
			a2 = 3.14 + angleBetweenVectors2(x2, c) ;
		else
			a2 = 3.14 - angleBetweenVectors2(x2, c);

		rotate_matrix(s1_pose_matrix, a1);
		rotate_matrix(s2_pose_matrix, a2);

		printf("ANGLES:\t%f\t%f\n", a1*180/3.14, a2*180/3.14);
	}



	if (s1_pose_matrix != NULL)
		displayObjectOnMarker(s1_pose_matrix, drawSimpleSnowman);
	if (s2_pose_matrix != NULL)
		displayObjectOnMarker(s2_pose_matrix, drawFatSnowman);	
}
